import java.util.Random;

import lejos.nxt.Button;
import lejos.nxt.MotorPort;
import lejos.nxt.NXTRegulatedMotor;
import lejos.nxt.SensorPort;
import lejos.nxt.TouchSensor;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.addon.AccelHTSensor;
import lejos.nxt.addon.RCXLightSensor;
import lejos.nxt.comm.RConsole;
import lejos.robotics.Accelerometer;
import lejos.util.Delay;

public class Robot1
{
	static int NUM_SAMPLES = 10;
	
	boolean finished;
	NXTRegulatedMotor motorA = new NXTRegulatedMotor(MotorPort.A);
	NXTRegulatedMotor motorB = new NXTRegulatedMotor(MotorPort.B);
	RCXLightSensor lightSensor_1 = new RCXLightSensor(SensorPort.S1);
	RCXLightSensor lightSensor_2 = new RCXLightSensor(SensorPort.S2);
	UltrasonicSensor usensor = new UltrasonicSensor(SensorPort.S3);
	double yArray[] = new double[NUM_SAMPLES];
	//AccelHTSensor accel = new AccelHTSensor(SensorPort.S3);
	double yAccum, yAccel, y;
	int distance = 0;
	

	//TouchSensor touch = new TouchSensor(SensorPort.S4);
	//AccelHTSensor accel = new AccelHTSensor(SensorPort.S1);
	int xAccler;
	
	public Robot1()
	{
//		RConsole.open();
		finished = false;
		lightSensor_1.setFloodlight(true);
		lightSensor_2.setFloodlight(true);
		int count = 0;
		motorA.setSpeed(720);
		motorB.setSpeed(720);
		
		while (!Button.ESCAPE.isDown() && !finished)
		{
			Integer value = lightSensor_1.getLightValue();
			Integer value2 = lightSensor_2.getLightValue();
			//xAccler = accel.getXAccel();
////			RConsole.println("S1: " + value.toString());
////			RConsole.println("S2: " + value2.toString());
			//yAccel = accel.getXAccel();
			//yArray[count % NUM_SAMPLES] = (yAccel > 200 ? -(1000.0 - yAccel) : yAccel);
			//this is a test
			//if(count++ < NUM_SAMPLES) continue;
			//yAccum = 0;
			//for(int i = 0; i < NUM_SAMPLES; i++) {
				//yAccum += yArray[i];
			//}

			//y = yAccum / NUM_SAMPLES;
			//RConsole.println("X: " + y);
//			/*
//			 * The if statements below will detect if the light sensors have
//			 * detected the black circle and center itself in the middle of the
//			 * circle and end the program once goal is reached.
//			 */
//
			if (value <= 20 && value2 <= 16) // both on black
			{
				motorA.rotate(360);
				motorB.rotate(360);
				finished = true;
			}
			else if (value <= 20 && value2 > 16) // s1 is on black s2 is not
			{
				// turn right
				motorA.stop(true);
				motorB.rotate(45, true);
			}
			else if (value > 20 && value2 <= 16) // s2 is on black s1 is not
			{
				// turn left
				motorB.stop(true);
				motorA.rotate(45, true);
			}
//
//			/*
//			 * What this should do is calculate the distance based upon the
//			 * rotation of the wheels. We've determine that the length of the
//			 * wheel is 9 1/2 inches and 1080 degrees is equal to 9 1/2 inches.
//			 * Which means that 360 degrees is equal to 3 inches. When we have
//			 * reached a certain distance, say a foot, it will check if the
//			 * sensors have detected the color black and if so, do nothing.
//			 * Otherwise, if both sensors have not detected anything, to
//			 * randomly change direction and repeat the process til both sensors
//			 * are on top of the black circle.
//			 */
//
			else
			{
				
//				if (touch.isPressed())
				if (usensor.getDistance() < 10)
				{// robot hits wall

					//if (touch.isPressed())// robot hits wall
				//{
					Random rand = new Random();
					
					switch(rand.nextInt(2)){
					case 1:
						//turn left
						motorA.rotate(-360, true);
						motorB.rotate(-360, true);
						Delay.msDelay(1000);
						motorA.rotate(560,true);
						motorB.rotate(-560, true);
						Delay.msDelay(2000);
						break;
					case 2: 
						//turn right
						motorA.rotate(-360, true);
						motorB.rotate(-360, true);
						Delay.msDelay(1000);
						motorA.rotate(-560, true);
						motorB.rotate(560, true);
						Delay.msDelay(2000);
						break;
					default:
						motorA.rotate(-360, true);
						motorB.rotate(-360, true);
						Delay.msDelay(1000);
						motorA.rotate(1150, true);
						motorB.rotate(-1150, true);
						Delay.msDelay(4500);
						break;
					}
				}
				else if()
				{
					
				}
				else
				{
					if( motorA.getPosition() > 5000 && motorB.getPosition() > 5000)
					{
						//turn
//						motorA.rotate(-360, true);
//						motorB.rotate(-360, true);
//						Delay.msDelay(1000);
						Random rand1 = new Random();
						
						/*switch(rand.nextInt)
						 * */
						
						motorA.rotate(-560, true);
						motorB.rotate(560, true);
						Delay.msDelay(2000);
//						distance = 0;
						motorA.resetTachoCount();
						motorB.resetTachoCount();
					}
					else
					{
						motorA.rotate(3*360, true);
						motorB.rotate(3*360, true);
//						distance += motorA.getPosition() - distance;
					}
				}
			}
		}
	}

	public static void main(String[] args)
	{
		Robot1 rover = new Robot1();
	}

}
